package org.nashua.tt151;

import edu.wpi.first.wpilibj.Timer;
import org.nashua.tt151.Controller.DualAction;
import org.nashua.tt151.RobotMap.Servo.Pos;

public class Actions {
    public static class Auto {
        private static double aim(int hoop){
            Main.led.set(true);
            return 0;
        }
        public static void aimlessShoot(double power){
            Main.shootTop.set(power);
            Main.shootBot.set(power);
            Timer.delay(1);
            Main.led.set(false);
        }
        public static void drive(double l, double r){
            Main.frontLeft.set(l);
            Main.frontRight.set(r);
            Main.backLeft.set(l);
            Main.backRight.set(r);
        }
        public static void drive(double l, double r, double power, boolean inverse){
            drive(l*power*(inverse?-1:1),-r*power*(inverse?-1:1));
        }
        public static void dropper(boolean down){
            Main.dropper.set(down?Pos.Dropper.DOWN:Pos.Dropper.UP);
        }
        public static void pickup(boolean on){
            Main.pickup.set(on?0.5:0);
        }
        public static void shoot(int hoop){
            aimlessShoot(aim(hoop));
        }
    }
    public static class Manual {
        private static void aim(){
            double x = Main.driver.getRawAxis(DualAction.Axis.LEFT_Y);
            double y = Main.driver.getRawAxis(DualAction.Axis.RIGHT_Y);
            Main.shootPitch.set(scaleValue(y<0?-y:Pos.Pitch.DEF,Pos.Pitch.DOWN,Pos.Pitch.UP));
            Main.shootYaw.set(scaleValue(x<0?-x:Pos.Yaw.DEF,Pos.Yaw.LEFT,Pos.Yaw.RIGHT));
        }
        public static void manualShoot(double power){
            while (Main.shooter.wasReleased(DualAction.Button.LEFT_TRIGGER)) aim();
            Auto.aimlessShoot(power);
        }
        public static void drive(double power,boolean inverse){
            double l = Main.driver.getRawAxis(inverse?DualAction.Axis.LEFT_Y:DualAction.Axis.RIGHT_Y)*power*(inverse?-1:1);
            double r = -Main.driver.getRawAxis(inverse?DualAction.Axis.RIGHT_Y:DualAction.Axis.LEFT_Y)*power*(inverse?-1:1);
            Main.frontLeft.set(l);
            Main.frontRight.set(r);
            Main.backLeft.set(l);
            Main.backRight.set(r);
        }
        public static void shoot(int hoop) {
            manualShoot(Auto.aim(hoop));
        }
    }
    private static double scaleValue(double orig, double min, double max){
        //scales a value to be between two given values (if orig is in range 0 to 1)
        return orig*(max-min)+min;
    }
}
